#include <ctype.h>
#include <string.h>
#include "mpu_9250_i2c.h"


//陀螺仪值
typedef struct
{
	short gx;
	short gy;
	short gz;
}MPU_Gyroscope;

//加速度值
typedef struct
{
	short ax;
	short ay;
	short az;
}MPU_Accelerometer;

//磁力计值
typedef struct
{
	short mx;
	short my;
	short mz;
}MPU_Magnetometer;


typedef struct
{
	MPU_Gyroscope gyroscope;
	MPU_Accelerometer accelerometer;
	MPU_Magnetometer magnetometer;
	short temperature;
	unsigned short rate;
	unsigned short lpf;
}mpu_9250_obj_s;

mpu_9250_obj_s g_obj =
{
	.rate = 10,
	.lpf  = 50,
};


int hd_mpu9250_init()
{
	mpu9250_HW_init();
	
	return 0;
}

int hd_mpu9250_exit()
{
	mpu9250_HW_exit();
	
    return 0;
}



int hd_mpu9250_get(mpu_9250_obj_s *p_obj)
{
	int ret = 0;
	
	p_obj->temperature = MPU_Get_Temperature();
	

    ret = MPU_Get_Gyroscope(&p_obj->gyroscope.gx,&p_obj->gyroscope.gy,&p_obj->gyroscope.gz);
	if(ret <= 0)
	{
		printf("MPU_Get_Gyroscope failed ret %d\n", ret);
	}

    ret = MPU_Get_Accelerometer(&p_obj->accelerometer.ax,&p_obj->accelerometer.ay,&p_obj->accelerometer.az);
	if(ret <= 0)
	{
		printf("MPU_Get_Accelerometer failed ret %d\n", ret);
	}	
	
	ret = MPU_Get_Magnetometer(&p_obj->magnetometer.mx,&p_obj->magnetometer.my,&p_obj->magnetometer.mz);
	if(ret <= 0)
	{
		printf("MPU_Get_Magnetometer failed ret %d\n", ret);
	}
	
	return ret;
}


int hd_mpu9250_set(mpu_9250_obj_s *p_obj)
{
	int ret = 0;
	ret = MPU_Set_Rate(p_obj->rate);
	if(ret < 0)
	{
		printf("MPU_Set_Rate failed rate %d\n", p_obj->rate);
		return ret;
	}
	
	ret = MPU_Set_LPF(p_obj->lpf);
	if(ret < 0)
	{
		printf("MPU_Set_LPF failed lpf %d\n", p_obj->lpf);
		return ret;
	}
	
	return 0;
}

void hd_mpu9250_print(mpu_9250_obj_s *p_obj)
{ 
	printf("MPU9250:\n");
	printf("\t temperature: %d\n", p_obj->temperature);
	printf("\t Gyroscope: gx=%d,gy=%d,gz=%d\n", p_obj->gyroscope.gx, p_obj->gyroscope.gy, p_obj->gyroscope.gz);
	printf("\t accelerometer: ax=%d,ay=%d,az=%d\n", p_obj->accelerometer.ax,p_obj->accelerometer.ay,p_obj->accelerometer.az);
	printf("\t magnetometer: mx=%d,my=%d,mz=%d\n", p_obj->magnetometer.mx,p_obj->magnetometer.my,p_obj->magnetometer.mz);
	
	return;
}

void main()
{
	int ret = 0;
	
	ret = hd_mpu9250_init();
	if(ret < 0)
	{
		return ;
	}

	hd_mpu9250_set(&g_obj);
	
	while(1)
	{
		sleep(1);

		if(hd_mpu9250_get(&g_obj) <= 0)
		{
			usleep(20000);
			continue;
		}
		
		
		hd_mpu9250_print(&g_obj);
		
		usleep(5000);
	}
	
	hd_mpu9250_exit();	
	
	printf("program exit\n");
}
